#include <string.h>
#include <stdint.h>
#include <avr/wdt.h>
#include <IRremote.h>
#include <TimerOne.h>

#define TIMES_PERIOD 10000  //定时时间间隔 10000us
#define LOG_INFO(s) Serial.println(s);
#define LOG_INFO_NONLN(ss) Serial.print(ss);
#define LOG_INT(var) Serial.println(var,DEC);
#define LOG_INT_NONLN(vars) Serial.print(vars,DEC);

#define DEBUG_LED

#define IR_USER_ADDR 0xFF00

#define IR_KEY_11 0x45
#define IR_KEY_12 0x46
#define IR_KEY_13 0x47
#define IR_KEY_21 0x44
#define IR_KEY_22 0x40
#define IR_KEY_23 0x43
#define IR_KEY_31 0x07
#define IR_KEY_32 0x15
#define IR_KEY_33 0x09
#define IR_KEY_41 0x16
#define IR_KEY_42 0x19
#define IR_KEY_43 0x0D
#define IR_KEY_51 0x0C
#define IR_KEY_52 0x18
#define IR_KEY_53 0x5E
#define IR_KEY_61 0x08
#define IR_KEY_62 0x1C
#define IR_KEY_63 0x5A
#define IR_KEY_71 0x42
#define IR_KEY_72 0x52
#define IR_KEY_73 0x4A

#define NOTE_C4 262 //C4 1（中央C大调）
#define NOTE_CS4 277 //C4#
#define NOTE_D4 294 //D4 2
#define NOTE_DS4 311 //D4#
#define NOTE_E4 330 //E4 3
#define NOTE_F4 349 //F4 4
#define NOTE_FS4 370 //F4#
#define NOTE_G4 392 //G4 5
#define NOTE_GS4 415 //G4#
#define NOTE_A4 440 //A4 6
#define NOTE_AS4 466 //A4#
#define NOTE_B4 494 //B4 7
#define NOTE_G3 196 //G3 ·5

int melody4[32]={
  NOTE_C4,NOTE_D4,NOTE_E4,NOTE_C4, //1
  NOTE_C4,NOTE_D4,NOTE_E4,NOTE_C4, //1
  NOTE_E4,NOTE_F4,NOTE_G4,
  NOTE_E4,NOTE_F4,NOTE_G4, //5
  NOTE_G4,NOTE_A4,NOTE_G4,NOTE_F4,NOTE_E4,NOTE_C4, //1
  NOTE_G4,NOTE_A4,NOTE_G4,NOTE_F4,NOTE_E4,NOTE_C4, //1
  NOTE_D4,NOTE_G3,NOTE_C4, //1
  NOTE_D4,NOTE_G3,NOTE_C4, //1
};

uint8_t noteDurations[32]={
  100,100,100,100,
  100,100,100,100,
  100,100,200,
  100,100,200,
  75,25,75,25,100,100,
  75,25,75,25,100,100,
  100,60,150,
  100,60,150,
};

enum _M_DIR
{
  M_STOP,
  M_FORWARD,
  M_BACK,
  M_LEFT,
  M_RIGHT,
}M_DIR;

enum _MODE_CTRL
{
  _MODE_IDLE,//0-空闲
  _MODE_REMOTE_MOVE,//1-遥控移动
  _MODE_AUTO_MOVE,//2-自动移动
  _MODE_LED_SHOW,//3-灯光展示
  _MODE_SLEAR,//4-激光炮
  _MODE_MUSIC,//5-音乐
  _MODE_FOLLOW_ME,//6-跟我走
  _MODE_NOT_COME,//7-别过来
  _MODE_FREEDOM,//8-自由表演
  _MODE_TEST,//9-自测
}MODE_CTRL;

uint8_t now_mode = _MODE_IDLE;

//任务结构
typedef struct _TASK_COMPONENTS
{
  int Run; //程序运行标记： 0-不运行，1-运行
  int Timer; //计时器
  int ItvTime;  //任务运行时间间隔
  void (*TaskHook)(void); //要运行的任务函数
}TASK_COMPONENTS;//任务定义,TASK_COMPONENTS是_TASK_COMPONENTS的别名

void TaskLogandShow();
void TaskTimersFlag();
void TaskPolling();
void TaskPeriod();

void TaskRemarks();
void TaskProcess();

static TASK_COMPONENTS TaskComps[]=
{
  {0,50,50,TaskLogandShow},  //灯表演
  {0,10,10,TaskTimersFlag},//运行状态指示
  {0,5,5,TaskPolling},//轮询
  {0,2,2,TaskPeriod},//20ms控制周期
};

//任务清单
typedef enum _TASK_LIST
{
  TASK_LOG_SHOW,
  TASK_TIMER_FLAG,
  TASK_POLLING,
  TASK_SERVO,
  TASKS_MAX   //总共可分配的定时任务数目
}TASK_LIST;


const int test_led_pin = 13;//状态指示灯引脚

void idel_task();
void (*func)(void)=idel_task;//函数指针

 
//马达的控制脚
const uint8_t F_motorA1_pin = 9;
const uint8_t F_motorB1_pin = 10;
const uint8_t B_motorA1_pin = 11;
const uint8_t B_motorB1_pin = 3;

//红外接收模块的信号脚
const int rev_pin =2;
IRrecv ir(rev_pin);//定义红外控制对象

//超声波控制脚
const uint8_t trig_pin = 17;
const uint8_t echo_pin = 18;
//RGB灯控制脚
const uint8_t R_led_pin = 7;
const uint8_t G_led_pin = 5;
const uint8_t B_led_pin = 6;
//蜂鸣器控制脚
const uint8_t Beep_pin = 8;//高电平不响
//激光灯控制脚
const uint8_t Laser_pin = 19;//低电平灭
bool enable_laser = 0;//1-打开激光炮
bool laser_flag = 0;//激光炮

//手电筒控制脚
const uint8_t Light_pin = 16;//低电平灭
bool enable_light = 0;//1-打开电筒
uint8_t light_cnt = 0;

//风扇控制脚
const uint8_t Fan_pin = 15;//低电平灭
bool enable_fan = 0;//1-打开风扇
uint8_t fan_cnt = 0;

int key_release_time = 0; //按键释放计时
uint8_t ir_command = 0;//当前红外遥控命令
uint8_t ir_command_last = 0;//命令被清空前的值
uint8_t move_action = M_STOP; //移动动作
uint8_t move_action_last = M_STOP; //上次移动动作

uint8_t led_style_index = 0; //1-8
uint8_t led_color_index = 0; //0-6
uint8_t index_delay = 0;
uint8_t count_time = 0;
int interval_time = 5;
uint8_t led_color[][7]={
  {0,0,0,0,0,0,0},
  {1,2,3,4,5,6,7},
  {1,0,1,0,1,0,2},
  {2,0,2,0,2,0,3},
  {3,0,3,0,3,0,4},
  {4,5,4,5,4,5,6},
  {5,6,5,6,5,6,7},
  {6,2,6,2,6,2,3},
  {7,3,7,3,7,3,4},
};

long distance_cm;

uint16_t Duration = NOTE_C4;

String voice_command = ""; //语音命令
String inputString = "";         // a String to hold incoming data
bool stringComplete = false;  // whether the string is complete
bool u_start = false; 

/*------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------*/
/*******************************************************************/
/*  系统初始化函数                                                     */
/*******************************************************************/
void setup()
{
  #ifdef DEBUG_LED 
    pinMode(test_led_pin,OUTPUT);
  #endif
  
  Motor_init();
  RGB_led_init();
  Laser_init();
  Beep_init();
  Light_init();
  Light_init();
  Fan_init();
  
  Serial.begin(9600);
  // reserve 50 bytes for the inputString:
  inputString.reserve(50);
  delay(500);
  ir.enableIRIn();//启动红外接收

  Timer1.initialize(TIMES_PERIOD);//初始化定时器为TIMES_PERIOD us
  Timer1.attachInterrupt( TaskRemarks );//设置中断回调函数

}
/*******************************************************************/
/*  主循环函数                                                     */
/*******************************************************************/
void loop()
{
  Rev_IRdata();
  Rev_Uartdata();
  TaskProcess(); 

  if(now_mode == _MODE_MUSIC) tone_play();
}

/*
接收红外遥控码
*/
void Rev_IRdata(void)
{
    static long ir_decodedRawData = 0;
    static uint8_t ir_command_temp = 0;
    
    if(ir.decode()){//有红外数据
      key_release_time = 2;//不能小于3, 重载清除按键码计数
      
      ir_decodedRawData = ir.decodedIRData.decodedRawData;//32位解码原始数据，长按按键时第二次数据为0
      ir_command_temp = ir.decodedIRData.command;//命令，长按或单按都一直正常
      ir_command = ir_command_temp;
      ir_command_last = ir_command;
//      Serial.println(ir_command_temp,HEX);
//      Serial.println(ir_decodedRawData,HEX);

      switch(ir_command_temp){
        case IR_KEY_41:{
          now_mode = _MODE_IDLE; 
          reset_sysstatus();
          RGB_led_style(now_mode);
        } break;
        case IR_KEY_42:{
          enable_laser = 1;//打开激光炮
        } break;
        case IR_KEY_43:{

        } break;
        case IR_KEY_51:{
          now_mode = _MODE_REMOTE_MOVE;//1-遥控移动
          reset_sysstatus();
          RGB_led_style(now_mode);
        }break;
        case IR_KEY_52:{
          now_mode = _MODE_AUTO_MOVE;//2-自动移动
          Serial.println("_MODE_AUTO_MOVE");
          reset_sysstatus();
          RGB_led_style(now_mode);
        }break;
        case IR_KEY_53:{
          now_mode = _MODE_LED_SHOW;//3-灯光展示
          reset_sysstatus();
          RGB_led_style(now_mode);
          led_style_index = 1;
        }break;
        case IR_KEY_61:{
          fan_cnt++;
          if(fan_cnt > 5){
            fan_cnt = 0;
            enable_fan = !enable_fan;//打开风扇  
            if(enable_fan) RGB_value_set(LOW,HIGH,LOW); 
            else RGB_value_set(HIGH,HIGH,HIGH);;            
          }
        }break;     
        case IR_KEY_62:{
          now_mode = _MODE_MUSIC;//5-音乐
          reset_sysstatus();
          RGB_led_style(now_mode);
        }break;   
        case IR_KEY_63:{
          now_mode = _MODE_FOLLOW_ME;//6-跟我走
          reset_sysstatus();
          RGB_led_style(now_mode);
        }break;    
        case IR_KEY_71:{
          now_mode = _MODE_NOT_COME;//7-别过来
          reset_sysstatus();
          RGB_led_style(now_mode);
        }break;     
        case IR_KEY_72:{
//          now_mode = _MODE_FREEDOM;//8-自由表演
//          reset_sysstatus();
//          RGB_led_style(now_mode);
          light_cnt++;
          if(light_cnt > 5){
            light_cnt = 0;
            enable_light = !enable_light;//打开电筒        
          }
        }break;  
        case IR_KEY_73:{
          now_mode = _MODE_TEST;//9-自测
          reset_sysstatus();
          RGB_led_style(now_mode);
        }break;    
        case IR_KEY_21:{
          if(now_mode == _MODE_LED_SHOW){
            index_delay++;
            if(index_delay > 2)
            {
              index_delay = 0;
              if(led_style_index > 1) led_style_index -= 1;
            }
          }
        }break;  
        case IR_KEY_22:{
          if(now_mode == _MODE_LED_SHOW){
            index_delay++;
            if(index_delay > 2)
            {
              index_delay = 0;
              if(led_style_index < 8) led_style_index += 1;
            }
          }
        }break;  
        case IR_KEY_31:{
          if(now_mode == _MODE_LED_SHOW){//闪烁变慢
            if(interval_time < 12) interval_time += 2;            
          }
          if(now_mode == _MODE_TEST){//测试变慢
            if(interval_time < 5) interval_time += 1;            
          }
        }break;   
        case IR_KEY_32:{
          if(now_mode == _MODE_LED_SHOW){//闪烁变快
            if(interval_time > 2) interval_time -= 2;
          }
         if(now_mode == _MODE_TEST){//测试变快
            if(interval_time > 0) interval_time -= 1;
          }
        }break;                                                    
        default: break;
      }
      ir_command_temp = 0;
      
      ir.resume();//下一次接收
    }
}

/*
接收语音命令码
*/
void Rev_Uartdata(void)
{
  if (stringComplete) {
//    Serial.println(inputString);
    // clear the string:
    voice_command = inputString;
    inputString = "";
    stringComplete = false;
  }  
}

/*******************************************************************/
/*  任务进程函数                                                     */
/*******************************************************************/
void TaskProcess()
{
   static unsigned char i=0;  //必须是static类型

   for(i=0;i<TASKS_MAX;i++) //逐个任务时间处理
   {
    if(TaskComps[i].Run)  //任务可以运行
    {
       TaskComps[i].TaskHook();  //调用任务函数
       TaskComps[i].Run = 0; //标志清0
    }
   }
}


/*******************************************************************/
/*  空任务函数                                                     */
/*******************************************************************/
void idel_task()
{
  
}
/*******************************************************************/
/*  看门狗初始化                                                     */
/*******************************************************************/
void wdt_init()
{
//  get_mcusr();
  wdt_enable(WDTO_8S);//设置看门狗溢出时间
}

/*******************************************************************/
/*  任务标记函数                                                     */
/*******************************************************************/
void TaskRemarks()
{
   static unsigned char i=0;   //必须是static类型

   for(i=0;i<TASKS_MAX;i++)   //逐个任务处理
   {
     if(TaskComps[i].Timer) //时间不为0
     {
      TaskComps[i].Timer--; //减去一个节拍
      if(TaskComps[i].Timer<=0)//时间减完了
      {
       TaskComps[i].Timer = TaskComps[i].ItvTime;  //恢复计时器值，重新下一次
       TaskComps[i].Run = 1; //任务可以运行
      }
     }
   }
}


/*******************************************************************/
/*  100ms, 定时计数 ，改变不同模式下的状态和标志位                                                    */
/*******************************************************************/
void TaskTimersFlag()
{
  static uint8_t led_delay=0;
  
  if(led_delay++ > 2){
  	led_delay = 0;    
  	blinkLED();
  }

  if(key_release_time > 0){
    key_release_time--;  //LOG_INT(key_release_time);
    if(!key_release_time){
      ir_command = 0;//清除按键命令

      if(ir_command_last == IR_KEY_42){
        Laser_OnOff(0);//关激光
        Beep_OnOff(0);//关炮声  
        ir_command_last = 0;  
        enable_laser = 0; 
        laser_flag = 0;  
      }

      if(now_mode == _MODE_REMOTE_MOVE) move_action = M_STOP;//遥控移动模式下，无按键按下时停止
    }
  }
  
  if(now_mode == _MODE_LED_SHOW){
      if(led_style_index > 0){
          if(++count_time > interval_time){
              count_time = 0;
              led_color_index += 1;
              if(led_color_index > 6) led_color_index = 0;               
          }
      }
  }

  if(enable_laser == 1){  
      laser_flag = !laser_flag;
      Laser_OnOff(laser_flag);//激光
      for(int i=0;i<20;i++) Beep_OnOff(laser_flag);//炮
  }

  Light_OnOff(enable_light);//电筒
  Fan_OnOff(enable_fan);//风扇
  
  voice_mode();//4-语音控制
}
/*******************************************************************/
/*  50ms,  执行不同模式下的函数                                                    */
/*******************************************************************/
void TaskPolling(void)
{
  if((now_mode == _MODE_FOLLOW_ME) || (now_mode == _MODE_NOT_COME) || (now_mode == _MODE_AUTO_MOVE)){
    distance_cm = Get_distance(trig_pin,echo_pin);
  }
   
}
/*******************************************************************/
/*  20ms,  周期控制函数                                                    */
/*******************************************************************/
void TaskPeriod(void)
{
  if(now_mode == _MODE_REMOTE_MOVE) remote_move_mode();//1-遥控移动
  else if(now_mode == _MODE_AUTO_MOVE) auto_move_mode();//2-自动移动
  else if(now_mode == _MODE_LED_SHOW) led_show_mode();//3-灯光展示
  //else if(now_mode == _MODE_SLEAR) voice_mode();//4-语音控制
  else  if(now_mode == _MODE_MUSIC) music_mode();//5-音乐
  else  if(now_mode == _MODE_FOLLOW_ME) follow_me_mode();//6-跟我走
  else  if(now_mode == _MODE_NOT_COME) not_come_mode();//7-别过来
  else  if(now_mode == _MODE_FREEDOM) freedom_mode();//8-自由表演
  else  if(now_mode == _MODE_TEST) test_mode();//9-自测
  else idle_stay();//0-空闲
}
/*******************************************************************/
/* 功能测试函数                                                   */
/*******************************************************************/
void Func_test(void)
{
    static uint8_t index = 0;

    RGB_led_style(index);

    index += 1;
    if(index >7) index=0;

    Laser_OnOff(index%2);
	  Beep_OnOff(index%2);
	
}

void remote_move_mode(void)
{
  
  if(ir_command == IR_KEY_12) move_action = M_FORWARD;
  else if(ir_command == IR_KEY_32) move_action = M_BACK;
  else if(ir_command == IR_KEY_21) move_action = M_LEFT;
  else if(ir_command == IR_KEY_23) move_action = M_RIGHT;

  if(move_action != move_action_last)//与上次移动动作不同
  {
    Motor_control(move_action);
    move_action_last = move_action;
  }
}

void auto_move_mode(void)
{
  static uint8_t move_times = 2;
  static bool event1_flag = false;//左后退1秒事件

  if(!event1_flag){//没有事件发生
    Motor_control(M_FORWARD);
  }
  
  if(distance_cm <= 25){    
    event1_flag = true;//标记事件1发生
    move_times = 2;//移动计时
  }

  if(event1_flag){
    if(move_times == 0){
      Motor_control(M_STOP);
      event1_flag = false;//清除标记事件1发生
      move_times = 2;//移动计时
    }
    else{
      move_times -= 1;
      Motor_control(M_RIGHT);
    }
  } 
}

void led_show_mode()
{
  if(led_style_index > 0) RGB_led_style(led_color[led_style_index][led_color_index]);
}

void voice_mode(void)
{
  if(voice_command == "$forward@") {Motor_control(M_FORWARD);voice_command = "";}
  else if(voice_command == "$back@") {Motor_control(M_BACK);voice_command = "";}
  else if(voice_command == "$left@") {Motor_control(M_LEFT);voice_command = "";}
  else if(voice_command == "$right@") {Motor_control(M_RIGHT);voice_command = "";}
  else if(voice_command == "$stop@"){
    Motor_control(M_STOP);
    voice_command = "";
    if(now_mode == _MODE_AUTO_MOVE)
    {
      now_mode = _MODE_IDLE;
      reset_sysstatus();
      RGB_led_style(now_mode);      
    }
  }
  else if(voice_command == "$open@") {enable_light = 1;voice_command = "";}//打开电筒
  else if(voice_command == "$close@") {enable_light = 0;voice_command = "";}//关闭电筒
  else if(voice_command == "$fan@") {enable_fan = 1;voice_command = "";}//打开风扇
  else if(voice_command == "$nofan@") {enable_fan = 0;voice_command = "";}//关闭风扇
  else if(voice_command == "$forward_s@") Motor_control_s(M_FORWARD);
  else if(voice_command == "$back_s@") Motor_control_s(M_BACK);
  else if(voice_command == "$left_s@") Motor_control_s(M_LEFT);
  else if(voice_command == "$right_s@") Motor_control_s(M_RIGHT);
}

void music_mode(void)
{
  static uint16_t i=0,index=0;
  
  if(++i > noteDurations[index]/5) {
    i=0;
    index += 1;
    if(index == 32) 
      index = 0;
    Duration = (1000000/melody4[index])/2;
    //Serial.println(melody4[index]);
    //Serial.println(Duration);
  }
}

void follow_me_mode(void)
{
  if(( distance_cm >= 15 ) && (distance_cm <= 20 )){
    Motor_control(M_FORWARD);
    RGB_led_style(0);
  }else{
    Motor_control(M_STOP);
    RGB_led_style(6);
  }
}

void not_come_mode(void)
{
  if(distance_cm <= 15 ){
    Motor_control(M_BACK);
    RGB_led_style(0);
  }else{
    Motor_control(M_STOP);
    RGB_led_style(7);
  }  
}

void freedom_mode(void)
{
  static uint8_t free_times = 60;
  static uint8_t event_flag = 1;

  if(event_flag == 1){
    if(free_times == 0){
      event_flag = 2;//标记事件2发生
      free_times = 60;//移动计时
    }
    else{
      free_times -= 1;

    }
  }
  if(event_flag == 2){
    if(free_times == 0){
      event_flag = 3;//标记事件3发生
      free_times = 25;//移动计时
    }
    else{
      free_times -= 1;

    }
  }
  if(event_flag == 3){
    if(free_times == 0){
      event_flag = 4;//标记事件3发生
      free_times = 60;//移动计时
    }
    else{
      free_times -= 1;
      Motor_control(M_STOP);
    }
  }
  if(event_flag == 4){
    if(free_times == 0){
      event_flag = 5;//标记事件5发生
      free_times = 60;//移动计时
    }
    else{
      free_times -= 1;

    }
  }
  if(event_flag == 5){
    if(free_times == 0){
      event_flag = 6;//标记事件6发生
      free_times = 25;//移动计时
    }
    else{
      free_times -= 1;

    }
  }  
  if(event_flag == 6){
    if(free_times == 0){
      event_flag = 1;//标记事件1发生
      free_times = 60;//移动计时
    }
    else{
      free_times -= 1;
      Motor_control(M_STOP);
    }
  }       
}

void test_mode(void)
{
  static uint8_t test_delay=0;

  if(test_delay++ >= interval_time){
    test_delay = 0;
    Func_test();
  }
}

void idle_stay(void)
{
  
}

void go_forward(void)
{
    digitalWrite(F_motorA1_pin,LOW);
    digitalWrite(F_motorB1_pin,HIGH);
    digitalWrite(B_motorA1_pin,HIGH);
    digitalWrite(B_motorB1_pin,LOW);   
}

void go_left(void)
{
    digitalWrite(F_motorA1_pin,HIGH);
    digitalWrite(F_motorB1_pin,LOW);
    digitalWrite(B_motorA1_pin,HIGH);
    digitalWrite(B_motorB1_pin,LOW);    
}

void go_right(void)
{
    digitalWrite(F_motorA1_pin,LOW);
    digitalWrite(F_motorB1_pin,HIGH);
    digitalWrite(B_motorA1_pin,LOW);
    digitalWrite(B_motorB1_pin,HIGH);    
}

void go_back(void)
{
    digitalWrite(F_motorA1_pin,HIGH);
    digitalWrite(F_motorB1_pin,LOW);
    digitalWrite(B_motorA1_pin,LOW);
    digitalWrite(B_motorB1_pin,HIGH);  
}

void stops(void)
{
    digitalWrite(F_motorA1_pin,HIGH);
    digitalWrite(F_motorB1_pin,HIGH);
    digitalWrite(B_motorA1_pin,HIGH);
    digitalWrite(B_motorB1_pin,HIGH);  
}

/*******************************************************************/
/*  电机控制函数                                                     */
/*******************************************************************/
void Motor_control(enum _M_DIR dir)
{

  switch(dir)
  {
    case M_STOP:{//停止
      stops();
    }break;
    case M_FORWARD:{//前进
      go_forward();
    }break;
    case M_BACK:{//后退
      go_back();
    }break;
    case M_LEFT:{//左转
      go_left();
    }break; 
    case M_RIGHT:{//右转
      go_right();
    }break;               
    default:{//停止
      stops();          
    }break;
  }
}

/*******************************************************************/
/*  电机控制函数                                                     */
/*******************************************************************/
void Motor_control_s(enum _M_DIR dir)
{
  static uint8_t delaycnt_move = 0;

  if(++delaycnt_move > 2){
    delaycnt_move = 0;
    dir = M_STOP;
    voice_command = "";
  }
  switch(dir)
  {
    case M_STOP:{//停止
      stops();
    }break;
    case M_FORWARD:{//前进
      go_forward();
    }break;
    case M_BACK:{//后退
      go_back();
    }break;
    case M_LEFT:{//左转
      go_left();
    }break; 
    case M_RIGHT:{//右转
      go_right();
    }break;               
    default:{//停止
      stops();          
    }break;
  }
}
void Motor_init(void)
{
  pinMode(F_motorA1_pin,OUTPUT);
  pinMode(F_motorB1_pin,OUTPUT);
  pinMode(B_motorA1_pin,OUTPUT);
  pinMode(B_motorB1_pin,OUTPUT);

  Motor_control(M_STOP);
}

int ledState = LOW;
volatile unsigned long blinkCount = 0; // use volatile for shared variables

void blinkLED(void)
{
  if (ledState == LOW) {
    ledState = HIGH;
    blinkCount = blinkCount + 1;  // increase when LED turns on
  } else {
    ledState = LOW;
  }
  digitalWrite(test_led_pin, ledState);
}

void RGB_led_init()
{
  pinMode(R_led_pin,OUTPUT);
  pinMode(G_led_pin,OUTPUT);
  pinMode(B_led_pin,OUTPUT);

  RGB_led_style(0);
}

void RGB_value_set(uint8_t R_level, uint8_t G_level, uint8_t B_level)
{
  digitalWrite(R_led_pin,R_level);
  digitalWrite(G_led_pin,G_level);
  digitalWrite(B_led_pin,B_level);
}

void RGB_led_style(uint8_t current_styles)
{
  static uint8_t last_styles = 20;

  if(last_styles != current_styles){
    switch(current_styles)
    {
      case 1:RGB_value_set(LOW,HIGH,HIGH);break;
      case 2:RGB_value_set(HIGH,LOW,HIGH);break;
      case 3:RGB_value_set(HIGH,HIGH,LOW);break;
      case 4:RGB_value_set(LOW,LOW,LOW);break;
      case 5:RGB_value_set(HIGH,LOW,LOW);break;
      case 6:RGB_value_set(LOW,HIGH,LOW);break;
      case 7:RGB_value_set(LOW,LOW,HIGH);break;
      default:RGB_value_set(HIGH,HIGH,HIGH);break;
    } 
    last_styles = current_styles;//更新上一次灯颜色   
  }

}

void Laser_init(void)
{
  pinMode(Laser_pin,OUTPUT);

  Laser_OnOff(0);
}

void Laser_OnOff(uint8_t on_off_flag)
{
  digitalWrite(Laser_pin,on_off_flag);
}

void Beep_init(void)
{
  pinMode(Beep_pin,OUTPUT);

  Beep_OnOff(0);//1是响
}

void Beep_OnOff(uint8_t on_off_flag)
{
  if(on_off_flag){
    digitalWrite(Beep_pin,LOW);    //设置输出低电平
    delayMicroseconds(1250); //延时PotBuffer值 us
    digitalWrite(Beep_pin,HIGH);     //设置输出高电平
    delayMicroseconds(1250);       //延时100us    
  }
  else{
    digitalWrite(Beep_pin,HIGH);     //设置输出高电平
  }
}

long Get_distance(const uint8_t trig, const uint8_t echo)
{
   long duration, cm;
   
   pinMode(trig, OUTPUT);
   digitalWrite(trig, LOW);
   delayMicroseconds(2);
   digitalWrite(trig, HIGH);
   delayMicroseconds(10);
   digitalWrite(trig, LOW);
   pinMode(echo, INPUT);
   duration = pulseIn(echo, HIGH);
   cm = duration / 29 / 2;

   return cm;
}

void Light_init(void)
{
  pinMode(Light_pin,OUTPUT);

  Light_OnOff(0);
}
void Light_OnOff(uint8_t on_off_flag)
{
  static uint8_t last_on_off_flag = 0;
  if(last_on_off_flag != on_off_flag)
  {
    digitalWrite(Light_pin,on_off_flag);  
    last_on_off_flag = on_off_flag;  
  }
}

void Fan_init(void)
{
  pinMode(Fan_pin,OUTPUT);

  Fan_OnOff(0);
}
void Fan_OnOff(uint8_t on_off_flag)
{
  static uint8_t last_on_off_flag = 0;
  if(last_on_off_flag != on_off_flag)
  {
    digitalWrite(Fan_pin,on_off_flag); 
    last_on_off_flag = on_off_flag;  
  }
}
/*
 myangle的范围是 0-180
*/
void servo_pulse(int servopin,int myangle)//定义一个脉冲函数
{
  static int pulsewidth;
  
  pulsewidth = (myangle*(500/45))+500;//将角度转化为500-2480 的脉宽值
  digitalWrite(servopin,HIGH);//将舵机接口电平至高
  delayMicroseconds(pulsewidth);//延时脉宽值的微秒数
  digitalWrite(servopin,LOW);//将舵机接口电平至低
  delay(20-pulsewidth/1000);
}

void reset_sysstatus(void)
{
    led_style_index = 0; //1-8
    led_color_index = 0; //0-6
    count_time = 0;
    interval_time = 5;
    Beep_OnOff(0);
    Laser_OnOff(0);
    Motor_control(M_STOP);
}

void tone_play()
{
  digitalWrite(Beep_pin,HIGH);
  delayMicroseconds(Duration);
  digitalWrite(Beep_pin,LOW);
  delayMicroseconds(Duration);
}

/*
  SerialEvent occurs whenever a new data comes in the hardware serial RX. This
  routine is run between each time loop() runs, so using delay inside loop can
  delay response. Multiple bytes of data may be available.
*/
void serialEvent() {
  while (Serial.available()) {
    // get the new byte:
    char inChar = (char)Serial.read();
    // add it to the inputString:
    if(inChar == '$'){
      u_start = true;
      inputString += inChar;      
    }else if(u_start)
    {
      inputString += inChar; 
    }
    // if the incoming character is a newline, set a flag so the main loop can
    // do something about it:
    if (inChar == '@') {
      stringComplete = true;
      u_start = false;
    }
  }
}
/*******************************************************************/
/*                                                     */
/*******************************************************************/
void TaskLogandShow()
{
//  LOG_INFO_NONLN("ir_command: ");Serial.println(ir_command,HEX);
//  LOG_INFO_NONLN("mode: ");LOG_INT(now_mode);
//  LOG_INFO_NONLN("led_style_index: ");LOG_INT(led_style_index);
//  LOG_INFO_NONLN("led_color_index: ");LOG_INT(led_color_index);
//  LOG_INFO_NONLN("interval_time: ");LOG_INT(interval_time);
//    LOG_INFO_NONLN("pulse_cnt: ");LOG_INT(pulse_cnt);
//    LOG_INFO_NONLN("pulse_width: ");LOG_INT(pulse_width);
//LOG_INFO_NONLN("enable_laser: ");LOG_INT(enable_laser);
//LOG_INFO_NONLN("event1_flag: ");LOG_INT(event1_flag);
//LOG_INFO_NONLN("event2_flag: ");LOG_INT(event2_flag);
//LOG_INFO_NONLN("distance_cm: ");LOG_INT(distance_cm);
}
